% Devin Koepl, Kevin Kemper

function sine_track
%	close( 'all' );
	clc
	
	save_plots = 0;
	
	f_lim = [0 0 0];
	
	
	for s = 1:3
		
		str = sprintf('./spring%d/sys%d.mat',s,s);
		sys = load(str);
		str = sprintf('./spring%d/ctrl%d.mat',s,s);
		ctrl = load(str);

		sys  = sys.sys;
		ctrl = ctrl.ctrl;

		tauc	= sys.tauc;
		GR		= sys.GR;

		K		= sys.K;
		B		= sys.B;
		I		= sys.I;
		OFFSET	= sys.OFFSET;

		w0	= sqrt(K/I);
		z	= B/(2*sqrt(K*I));
		wd	= w0*sqrt(1-z^2);
		fd	= wd/(2*pi);


		%%%%%%
		A11 = 0;
		A12 = 1;
		A21	= -K/I;
		A22 = -B/I;

		B1 = 0;
		B2 = 1/I;

		C11 = 1;
		C12 = 0;
		C21 = 0;
		C22 = 1;

		INVCB1 = 0;
		INVCB2 = 1/B2;
		%%%%%%

	%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
	% Run controllers

		n = 100;

		f = logspace(0,2,n);

	%  	f = 10;

		a		= ctrl.a;
		b		= ctrl.b;
		k1		= ctrl.k1;
		k2		= ctrl.k2;
		ke1		= ctrl.ke1;
		ke2		= ctrl.ke2;
		trq_lim = ctrl.trq_lim;

		x0 = [0 0];


		for fi = 1:n
			fprintf('fi %d\n',fi);

			w		= 2*pi*f(fi);

			options		= odeset('RelTol',1e-3,	'MaxStep',1/f(fi));

			%%%%%%
			[t, x] = ode45(@dynamics, [0, 4/f(fi)], x0 ,options);

			mtr_trq = t;
			for i = 1:length(t);
				mtr_trq(i)		= -u(t(i),x(i,1),x(i,2));
			end
			spring_trq	= x(:, 1)*K;

			ti = find(t>=2/f(fi),1);
			%%%%%%

			amps(fi,s) = max(abs(spring_trq(ti:end)));

			mag(fi,s) = 20*log10(amps(fi,s)/a);

			if (f_lim(s) == 0)
		% 		if (sum(abs(mtr_trq)>=trq_lim) >= ceil(length(t)*0.01))
				if (max(abs(mtr_trq(ti:end)))>=trq_lim)
					f_lim(s) = f(fi);
		% 			figure
		% 			hold on
		% 			plot([t(1) t(end)],[trq_lim trq_lim],'k--');
		% 			plot([t(1) t(end)],[-trq_lim -trq_lim],'k--');
		% 			plot(t,mtr_trq);
		% 			plot(t,spring_trq,'r');
				end
			end

		end
	end
	
	
	%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
	h = figure;
	set(h,'position',[ 100 100 800 400])

	semilogx([1 100], [0 0],'k--','linewidth', 2);
	hold on

	semilogx([1 100], [3 3],'k--','linewidth', 1);
	semilogx([1 100], [-3 -3],'k--','linewidth', 1);

	% semilogx([fd fd], [-3 3],'g','linewidth', 2);
	% text(fd,max(amps)- max(amps)*0.05,'f_d \rightarrow','FontSize',12, 'HorizontalAlignment', 'right');
	if (f_lim(1) ~= 0)
		semilogx([f_lim(1) f_lim(1)], [min(min(mag)) 3],'g','linewidth', 2);
		str = sprintf('$%2.3f \\  Hz$',f_lim(1));
		text(f_lim(1),-19,str,'FontSize',12, 'HorizontalAlignment', 'right','interpreter','latex');
	end
	if (f_lim(2) ~= 0)
		semilogx([f_lim(2) f_lim(2)], [min(min(mag)) 3],'g','linewidth', 2);
		str = sprintf('$%2.3f \\  Hz$',f_lim(2));
		text(f_lim(2),-17,str,'FontSize',12, 'HorizontalAlignment', 'right','interpreter','latex');
	end
	if (f_lim(3) ~= 0)
		semilogx([f_lim(3) f_lim(3)], [min(min(mag)) 3],'g','linewidth', 2);
		str = sprintf('$\\ %2.3f \\  H z$',f_lim(3));
		text(f_lim(3),-19,str,'FontSize',12, 'HorizontalAlignment', 'left','interpreter','latex');
	end

	pmag = semilogx(f,mag,'linewidth', 3);

	axis tight
	
	legend(pmag,'Soft', 'Medium', 'Stiff','location','southwest');

	xlabel(	'\textbf{Frequency} $\left( H\ z \hspace{1 mm}\right)$',...
			'interpreter','latex','fontsize',16);

	ylabel(	'\textbf{Magnitude} (d B )',...
			'interpreter','latex','fontsize',16);

	set(gca,'Position',[ 0.10 0.15 0.85 0.8])

	str = sprintf('/home/drl/projects/jhurst/current/Kemper_journal/images/bodeplot_sim.pdf');
	if (save_plots == 1)
		save2pdf(str);
	end
	
	return
	
	%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
	h = figure;
	set(h,'position',[ 100 100 400 400])
	hold on
	% str = sprintf('Spring %d: Torques',springN);
	% title(str)

	plot([t(1) t(end)], [a a], 'k--','linewidth', 1);
	plot([t(1) t(end)], [-a -a], 'k--','linewidth', 1);
	plot([t(1) t(end)], [b b], 'k--','linewidth', 1);

	pt = plot(t,mtr_trq,'g','linewidth', 2);

	pd = plot(t,(b+a*sin(w*(t))),'r--','linewidth',3);

	pm = plot(t,spring_trq,'linewidth', 2);


	legend([pd; pm; pt],'Desired', 'Measured', 'Motor Torque')
	axis tight

	xlabel(	'\textbf{Time} $\left( s \hspace{1 mm}\right)$',...
			'interpreter','latex','fontsize',16);

	ylabel(	'\textbf{Torque} $\left( N \cdot m \hspace{1 mm}\right)$',...
			'interpreter','latex','fontsize',16);

	set(gca,'Position',[ 0.15 0.15 1 0.9])

% 	str = sprintf('/home/drl/projects/jhurst/current/Kemper_journal/images/track%d.pdf', springN);
% 	if (save_plots == 1)
% 		save2pdf(str);
% 	end
	
	
	%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 	function trq = u(t,x1,x2)
% 		e = ydt(t) - C * x; 
% 		u = - K * x + (C * B) \ ( yddt(t) - C*(A - B*K)*x + Ke * e );

		
		yd1	= (b+a*sin(w*t))/K;
		yd2	= (a*w*cos(w*t))/K;
		ydd	= (a*w*w*-sin(w*t))/K;

		err1 = yd1 -  (C11*x1 + C12*x2);
		err2 = yd2 -  (C21*x1 + C22*x2);
		
%		C*(A-B*K);
		bk11 = B1*k1;
		bk12 = B1*k2;
		bk21 = B2*k1;
		bk22 = B2*k2;
		
		abk11 = A11-bk11;
		abk12 = A12-bk12;
		abk21 = A21-bk21;
		abk22 = A22-bk22;
		
		cabk11 = C11*abk11+C12*abk21;
		cabk12 = C11*abk12+C12*abk22;
		cabk21 = C21*abk11+C22*abk21;
		cabk22 = C21*abk12+C22*abk22;
		
		tmp1 = ydd - (cabk11*x1+cabk12*x2) + ke1*err1 + ke2*err2;
		tmp2 = ydd - (cabk21*x1+cabk22*x2) + ke1*err1 + ke2*err2;

		trq = -(k1*x1+k2*x2) + INVCB1*tmp1 + INVCB2*tmp2;
		
		if (trq > trq_lim)
			trq = trq_lim;
		elseif (trq < -trq_lim)
			trq = -trq_lim;
		end

	end
	
	
 	function xdot = dynamics(t, x)
% 		e = ydt(t) - C * x; 
% 		u = - K * x + (C * B) \ ( yddt(t) - C*(A - B*K)*x + Ke * e );
		
 		tau = u(t,x(1),x(2));
%		tau = 0;
		xdot = [A11, A12; A21, A22] * x + [B1;B2] * tau;
	end


end
